#!/usr/bin/env python3
from pyhal import *
import time
from struct import *
import os
from raster import *

theta = 0.005
timeout = 0.25

def fleq(a, b):
    return (a - b) < theta
    

def assertIdle(pin):
    assert pin['fault'].value == 0, "Fault detected"
    assert pin['state'].value == 0, "raster not idle"

def waitEnabled(pin):
    stop = time.process_time() + timeout
    while not pin['enabled'].value:
        assert pin['fault'].value == 0, "Fault occurred. Code {0}".format(fault_code.value)
        assert time.process_time() < stop, "Timeout while waiting" 

        
def waitDisabled():
    stop = time.process_time() + timeout
    while enabled.value:
        assert fault.value == 0, "Fault occurred. Code {0}".format(fault_code.value)
        assert time.process_time() < stop, "Timeout while waiting"

def waitFault(pin):
    stop = time.process_time() + timeout
    while not pin['fault'].value:
        assert time.process_time() < stop, "Timeout while waiting"

def waitIdle():
    stop = time.process_time() + timeout
    while pin['state'].value != 0:
        assert fault.value == 0, "Fault occurred. Code {0}".format(fault_code.value)
        assert time.process_time() < stop, "Timeout while waiting"

def resetRaster(prog, pin):
    stop = time.process_time() + timeout
    prog.run.value = 0
    pin['reset'].value = 1
    while pin['state'].value != 0:
        assert time.process_time() < stop, "Timeout while waiting"
    pin['reset'].value = 0
    assertIdle(pin)


def testValidBegin(prog, pin):
    resetRaster(prog, pin)
    prog.port.write("123.45;4;1.0;10;0000000000")
    prog.run.value = 1

    waitEnabled(pin)

    assert fault.value == 0, "Fault detected."
    assert fleq(offset.value, 123.45), "Offset was not set correctly"
    assert bpp.value == 4, "BPP was not set correctly, Given:{0}".format(bpp.value)
    assert fleq(ppu.value, 1.0), "PPU was not set correctly"
    assert count.value == 10, "count was not set correctly"

    prog.run.value = 0
    waitIdle()
    assertIdle(pin)


def testInvalidOffset(prog, pin):
    resetRaster(prog, pin)
    prog.port.write("FOOB;41.0;2;00")
    prog.run.value = 1

    waitFault(pin)
    assert pin['fault'].value == 1, "Fault not detected"
    assert pin['fault_code'].value == FaultCodes.InvalidOffset.value


def testInvalidBPP(prog, pin):
    for bpp in [0, 2, 10, 14, 54, -50.0]:
        resetRaster(prog, pin)
        prog.port.write("123.45;{0};1.0;2;00".format(bpp))
        prog.run.value = 1
        waitFault(pin)
        assert pin['fault_code'].value == FaultCodes.InvalidBPP.value

def testValidBPP(prog, pin):
    for bpp in [4, 8, 12, 16, 20, 24, 28, 32]:
        resetRaster(prog, pin)
        prog.port.write("123.45;{0};1.0;2;{1}".format(bpp, "0" * 2 * int(bpp/4)))
        prog.run.value = 1
        waitEnabled(pin)
       
def testInvalidPPU(prog, pin):
    for ppu in [-1.0, 0.0]:
        resetRaster(prog, pin)
        prog.port.write("123.45;4;{0};2;00".format(ppu))
        prog.run.value = 1
        waitFault(pin)
        assert pin['fault_code'].value == FaultCodes.InvalidPPU.value

def testValidPPU(prog, pin):
    for tppu in [0.01, 1423.2343, 1000]:
        resetRaster(prog, pin)
        prog.port.write("123;4;{0};2;00".format(tppu))
        prog.run.value = 1
        waitEnabled(pin)
        assert fleq(pin['ppu'].value, tppu), "PPU failed to be set"

def testValidCount(prog, pin):
    for tcount in [2, 10, 500]:
        resetRaster(prog, pin)
        prog.port.write("-156;4;1.0;{0};{1}".format(tcount, "0" * tcount))
        prog.run.value = 1
        waitEnabled(pin)
        assert pin['count'].value == tcount, "Count was not valid"

def testInvalidCount(prog, pin):
    for tcount in [0, 1, -1, -100]:
        resetRaster(prog, pin)
        prog.port.write("2334.1223;4;1.0;{0};00000000".format(tcount))
        prog.run.value = 1
        waitFault(pin)
        assert pin['fault_code'].value == FaultCodes.InvalidCount.value, "Not expected fault code {0}. tcount {1}".format(fault_code.value, tcount)

def testWrongDirection(prog, pin):
    resetRaster(prog, pin)
    position.value = -10
    prog.port.write("10;4;1.0;10;0123456789")
    prog.run.value = 1
    waitEnabled(pin)

    position.value = -11
    waitFault(pin)
    assert fault_code.value == FaultCodes.WrongDirection.value, "Expected fault WrongDirection. Got {0}".format(fault_code.value)

    resetRaster(prog, pin)
    position.value = 10
    prog.port.write("-10;4;1.0;10;0123456789")
    prog.run.value = 1
    waitEnabled(pin)
    position.value = 11
    waitFault(pin)
    assert fault_code.value == FaultCodes.WrongDirection.value, "Expected fault WrongDirection. Got {0}".format(fault_code.value)
   

def testProgram(prog, pin, pos, offset, bpp, dpu, program, data):
    pin['position'].value = pos
    prog.begin(offset, bpp, dpu, int(len(program)/(bpp/4)))
    prog.data(program)

    prog.start()
    for (pos, pow) in data:
        pin['position'].value = pos
        time.sleep(0.001)
        assert (pin['output'].value - pow) < theta, "output at position {0} should be {1}. Got {2}".format(pos, pow, output.value)
        #once prosition is exceeded the program should be finished and raster should reset
    assert pin['enabled'].value == True, "raster should still be enabled"
    prog.stop()


def main():
    try:
        c = component("test")

        pin = {
            'reset': c.pinNew("reset", halType.BIT, pinDir.OUT),
            'enabled': c.pinNew("enabled", halType.BIT, pinDir.IN),
            'position': c.pinNew("position", halType.FLOAT, pinDir.OUT),
            'output': c.pinNew("output", halType.FLOAT, pinDir.IN),
            'offset': c.pinNew("offset", halType.FLOAT, pinDir.IN),
            'bpp': c.pinNew("bpp", halType.SIGNED, pinDir.IN),
            'ppu': c.pinNew("ppu", halType.FLOAT, pinDir.IN),
            'count': c.pinNew("count", halType.SIGNED, pinDir.IN),
            'fault': c.pinNew("fault", halType.BIT, pinDir.IN),
            'fault_code': c.pinNew("fault-code", halType.SIGNED, pinDir.IN),
            'state': c.pinNew("state", halType.SIGNED, pinDir.IN),
            'bitmap_position': c.pinNew("bitmap-position", halType.FLOAT, pinDir.IN),
            'current_pixel_index': c.pinNew("current-pixel-index", halType.SIGNED, pinDir.IN),
            'current_pixel_value': c.pinNew("current-pixel-value", halType.FLOAT, pinDir.IN),
            'previous_pixel_value': c.pinNew("previous-pixel-value", halType.FLOAT, pinDir.IN),
            'fraction': c.pinNew("fraction", halType.FLOAT, pinDir.IN),
        }
        c.ready()

        prog = RasterProgrammer("programmer")

        #instantiate the raster component,
        #add it to a thread and link all signals from
        #the test component here
        assert os.system('halcmd -f raster.hal') == 0, "raster.hal script failed"

        testInvalidOffset(prog, pin)
        testInvalidBPP(prog, pin)
        testValidBPP(prog, pin)
        testInvalidPPU(prog, pin)
        testValidPPU(prog, pin)
        testValidCount(prog, pin)
        testInvalidCount(prog, pin)
        #testWrongDirection()
        resetRaster(prog, pin)
        testProgram(prog, pin, -1.0,
                    1.0,
                    4,
                    1.0,
                    "0E",
                    [(-0.5, -1.0),
                     (0.0, 0.0),
                     (0.25, 25.0),
                     (0.5, 50.0),
                     (0.75, 75.0),
                     (1.0, 100.0),
                     (1.1, -1.0)])

        testProgram(prog, pin, 2.0,
                    -1.0,
                    4,
                    1.0,
                    "0E",
                    [(1.1, -1.0),
                     (1.0, 0.0),
                     (0.75, 25.0),
                     (0.5, 50.0),
                     (0.25, 75.0),
                     (0.0, 100.0),
                     (-0.5, -1.0)])

        testProgram(prog, pin, -1.0,
                    1.0,
                    12,
                    1.0,
                    "000FFE",
                    [(-0.5, -1.0),
                     (0.0, 0.0),
                     (0.25, 25.0),
                     (0.5, 50.0),
                     (0.75, 75.0),
                     (1.0, 100.0),
                     (1.1, -1.0)])
    except Exception as e:
        print("error: Test failed: %s" % str(e))
        return 1
    finally:
        c.exit()
        prog.exit()
        os.system('halrun -U')
    return 0

if __name__ == "__main__":
    exit(main())
